using System.Collections.Generic;
using Unity.MLAgents;
using UnityEngine;
public enum Team
{
    Red = 0,
    Blue = 1
}

public class SatelliteEnvController : MonoBehaviour
{
    [System.Serializable]
    public class PursuerInfo
    {
        public AgentSatelliteP Agent;
        [HideInInspector]
        public Vector3 StartingPos;
        [HideInInspector]
        public Quaternion StartingRot;
        [HideInInspector]
        public Rigidbody Rb;
    }

    [System.Serializable]
    public class EvaderInfo
    {
        public AgentSatelliteE Agent;
        [HideInInspector]
        public Vector3 StartingPos;
        [HideInInspector]
        public Quaternion StartingRot;
        [HideInInspector]
        public Rigidbody Rb;
    }

    /// <summary>
    /// Max Academy steps before this platform resets
    /// </summary>
    /// <returns></returns>
 
    //0.02s   10000*0.02=200s

    [Tooltip("Max Environment Steps")] public int MaxEnvironmentSteps = 10000;

    //List of Agents On Platform
    public List<PursuerInfo> PursuerList = new List<PursuerInfo>();
    public List<EvaderInfo> EvaderList = new List<EvaderInfo>();

    //private SoccerSettings m_SoccerSettings;

    private SimpleMultiAgentGroup m_RedPursuerGroup;
    private SimpleMultiAgentGroup m_BlueEvaderGroup;

    private int m_ResetTimer;




    void Start()
    {

      //  m_SoccerSettings = FindObjectOfType<SoccerSettings>();
        // Initialize TeamManager
        m_RedPursuerGroup = new SimpleMultiAgentGroup();
        m_BlueEvaderGroup = new SimpleMultiAgentGroup();
       
        foreach (var item in PursuerList)
        {
            item.StartingPos = item.Agent.transform.localPosition;
            item.StartingRot = item.Agent.transform.localRotation;
            item.Rb = item.Agent.GetComponent<Rigidbody>();
            m_RedPursuerGroup.RegisterAgent(item.Agent);
        } 
        foreach (var item in EvaderList)
        {
            item.StartingPos = item.Agent.transform.localPosition;
            item.StartingRot = item.Agent.transform.localRotation;
            item.Rb = item.Agent.GetComponent<Rigidbody>();
            m_BlueEvaderGroup.RegisterAgent(item.Agent);
        } 

        ResetScene();
    }
      
    void FixedUpdate()
    {
        
        m_ResetTimer += 1;
        
        var agentred = PursuerList[0];
        var agentblue = EvaderList[0];
        // 过程奖励和终端奖励
        // 过程奖励   距离 速度夹角？
        Vector3 posagentred = agentred.Agent.transform.localPosition;
        Vector3 posagentblue = agentblue.Agent.transform.localPosition;
        Vector3 relaPos = posagentblue-posagentred;

        Vector3 velagentred = agentred.Rb.velocity;
        Vector3 velagentblue = agentblue.Rb.velocity;
              
        float distancePE = relaPos.magnitude;
        
        //速度是它自己的速度，可别再相对速度了
        float speed = Vector3.Dot(relaPos.normalized, velagentred.normalized);
        //m_RedPursuerGroup.AddGroupReward(speed*0.01f);
        //Debug.Log("relapos:"+relaPos.normalized+"relavel:"+relavel.normalized);
        //Debug.Log("点积:"+speed);
        //Debug.Log("velagentblue:"+velagentblue+"posagentblue:"+posagentblue);
    

        int maxD=1000; // 假设1000为最大可能距离
        // 从向量平行到向量共线，平行也可以了
        m_RedPursuerGroup.AddGroupReward(speed*0.00001f);//1-distancePE/maxD
        m_BlueEvaderGroup.AddGroupReward(0);//distancePE/maxD
        //Debug.Log(1-distancePE/maxD);
        //Debug.Log("evaderReward:"+distancePE/maxD);
        
        
        double capD = 10;  // 捕获界限
        // 捕获结束判断
        if (distancePE <= capD)
        {
            m_RedPursuerGroup.AddGroupReward(1 - (float)m_ResetTimer / MaxEnvironmentSteps);
            m_BlueEvaderGroup.AddGroupReward(-1);

            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("1");
        }
        // 未捕获到结束判断
        if (m_ResetTimer >= MaxEnvironmentSteps && MaxEnvironmentSteps > 0)
        {
            m_BlueEvaderGroup.AddGroupReward(1+ distancePE/maxD);
            m_RedPursuerGroup.AddGroupReward(-distancePE/maxD);
            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("2");
        }
        // 未捕获到结束判断
        if (distancePE > maxD)
        {
            m_BlueEvaderGroup.AddGroupReward(2);
            m_RedPursuerGroup.AddGroupReward(-2);
            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("3");
        }
    }


    public void ResetScene()
    {
        m_ResetTimer = 0;

        // 红方蓝方可以用相同的策略吗，一个是追一个是逃
        //红方初始位置
        foreach (var item in PursuerList)
        {
            var randomPosX = Random.Range(-20f, 20f);
            var randomPosY = Random.Range(-20f, 20f);
            var randomPosZ = Random.Range(-20f, 20f);
            var newStartPos = item.Agent.initialPos + new Vector3(randomPosX, randomPosY, randomPosZ);           
            var newRot = Quaternion.Euler(0, 0, 0);
            item.Agent.transform.SetPositionAndRotation(newStartPos, newRot);
            item.Rb.velocity = Vector3.zero;
            item.Rb.angularVelocity = Vector3.zero;
        }
        foreach (var item in EvaderList)
        {
            var randomPosX = Random.Range(-5f, 5f);
            var randomPosY = Random.Range(-5f, 5f);
            var randomPosZ = Random.Range(-10f, 10f);
            var newStartPos = item.Agent.initialPos;           
            var newRot = Quaternion.Euler(0, 0, 0);
            item.Agent.transform.SetPositionAndRotation(newStartPos, newRot);
            item.Rb.velocity = Vector3.zero;
            item.Rb.angularVelocity = Vector3.zero;
        }

    }
}
